#include "motion_cmd.h"
#include <string.h>
#include "fatfs.h"
#include "ffconf.h"
#include "bsp.h"
#include "app.h"
#include "vender.h"
#include "timestamp.h"
#include "netif.h"

FIL* file_update;
uint8_t res_http;
uint32_t pack_num = 0;
uint32_t func = 0;
uint32_t motion_req_proc(uint8_t *rcv_buf,uint8_t *send_buf,uint32_t len)
{
    uint32_t sendlen = 0;
    switch (rcv_buf[0])
    {
    case MOTION_MOVE_LEN:
    {
        if(action_func)
            break;
        uint8_t axis = rcv_buf[1];
        int64_t dest;
        memcpy(&dest,&rcv_buf[2],8);
        uint32_t speed;
        memcpy(&speed,&rcv_buf[10],4);
        mt_movelen_r_comb(axis,dest,speed);
    }
        /* code */
        break;
    case MOTION_MOVE_LENEX:
    {
        if(action_func)
            break;
        uint8_t axis = rcv_buf[1];
        float dest;
        memcpy(&dest,&rcv_buf[2],4);
        float speed;
        memcpy(&speed,&rcv_buf[6],4);
        uint32_t speed_hz = speed/25.4f*axis_info[axis].axis_param.resolution;
        int64_t dest_hz = dest/25.4f*axis_info[axis].axis_param.resolution;
        mt_movelen_r_comb(axis,dest_hz,speed_hz);
    }
        /* code */
        break;
    case MOTION_STOP:
    {
        if(action_func)
            break;
        uint8_t axis = rcv_buf[1];
        for(uint8_t i=0;i<AXIS_MAX;i++)
        {
            if(axis&BIT(i))
            {
                mt_stop(i,0);
            }
        }
    }
        /* code */
        break;
    case MOTION_GET_PWM_COOR:
    {
        uint8_t axis = rcv_buf[1];
        int64_t coor = mt_get_pwm_coor(axis);
        send_buf[4] = axis;
        memcpy(&send_buf[5],(uint8_t *)&coor,sizeof(int64_t));
        sendlen = 9;
    }
        /* code */
        break;
    case MOTION_GET_COOR:
    {
        uint8_t axis = rcv_buf[1];
        int64_t coor = mt_get_axis_coor(axis);
        send_buf[4] = axis;
        memcpy(&send_buf[5],(uint8_t *)&coor,sizeof(int64_t));
        sendlen = 9;
    }
        /* code */
        break;
    case MOTION_MOVE_TO:
    {
        if(action_func)
            break;
        uint8_t axis = rcv_buf[1];
        int64_t dest;
        memcpy(&dest,&rcv_buf[2],8);
        uint32_t speed;
        memcpy(&speed,&rcv_buf[10],4);
        mt_moveto_r_comb(axis,dest,speed);
    }
        break;
    case MOTION_MOVE_TOEX:
    {
        if(action_func)
            break;
        uint8_t axis = rcv_buf[1];
        float dest;
        memcpy(&dest,&rcv_buf[2],4);
        float speed;
        memcpy(&speed,&rcv_buf[6],4);
        uint32_t speed_hz = speed/25.4f*axis_info[axis].axis_param.resolution;
        int64_t dest_hz = dest/25.4f*axis_info[axis].axis_param.resolution;
        mt_moveto_r_comb(axis,dest_hz,speed_hz);
    }
        break;
    case MOTION_UPDATE_START:
    {
        uint8_t file_type = rcv_buf[1];
        if(file_update == NULL)
        {
            file_update = (FIL*)pvPortMalloc(sizeof(FIL));
        }
        if (func == 1)
        {
            f_close(file_update);
        }
        switch (file_type)
        {
        case 1:
            res_http = f_open(file_update, "0:Update.dat", FA_OPEN_ALWAYS | FA_CREATE_ALWAYS | FA_WRITE);
            func = 1;
            pack_num = 1;
            pr_info("Update.dat start %d\n",res_http);
            break;
        case 2:
            res_http = f_open(file_update, "0:motion.json", FA_OPEN_ALWAYS | FA_CREATE_ALWAYS | FA_WRITE);
            func = 1;
            pack_num = 1;
            pr_info("motion.json start %d\n",res_http);
            break;
        case 3:
            res_http = f_open(file_update, "0:Update_bak.dat", FA_OPEN_ALWAYS | FA_CREATE_ALWAYS | FA_WRITE);
            func = 1;
            pack_num = 1;
            pr_info("Update.dat start %d\n",res_http);
            break;
        default:
            break;
        }
    }
        break;
    case MOTION_UPDATE_DATA:
    {
        if (res_http == 0)
        {
            uint32_t bw;
		    uint8_t res = f_write(file_update, &rcv_buf[1], len-1, &bw);
            pack_num++;
            pr_info("pack_num %d res %d\n", pack_num,res);
        }
    }
        break;
    case MOTION_UPDATE_END:
    {
        if (func)
        {
            pr_info("update end\n");
            f_close(file_update);
            func = 0;
        }
    }
        break;
    case MOTION_CLEAN:
    {
        if(action_func)
            break;
        action_func = MOTION_CLEAN;
    }
        break;
    case MOTION_DOWET:
    {
        if(action_func)
            break;
        action_func = MOTION_DOWET;
    }
        break;
    case MOTION_OUTWET:
    {
        if(action_func)
            break;
        action_func = MOTION_OUTWET;
    }
        break;
    case MOTION_IS_MOVING:
    {
        uint8_t axis = rcv_buf[1];
        send_buf[4] = axis;
        if(axis == 0xff)
        {
            send_buf[5] = mt_ismoving(AXIS1);
            send_buf[6] = mt_ismoving(AXIS2);
            send_buf[7] = mt_ismoving(AXIS3);
            send_buf[8] = mt_ismoving(AXIS4);
            send_buf[9] = mt_ismoving(AXIS5);
            send_buf[10] = mt_ismoving(AXIS6);
            send_buf[11] = mt_ismoving(AXIS7);
            send_buf[12] = mt_ismoving(AXIS8);
            sendlen = 9;
        }
        else
        {
            send_buf[5] = mt_ismoving(axis);
            sendlen = 2;
        }
    }
        break;
    case MOTION_IO_GET_IN:
    {
        for(uint8_t i=IN1;i<INMAX;i++)
        {
            send_buf[4+i-1] = bsp_get_in(i);
        }
        sendlen = INMAX-1;
    }
        break;
    case MOTION_IO_GET_OUT:
    {
        for(uint8_t i=OUT1;i<OUTMAX;i++)
        {
            send_buf[4+i-1] = bsp_get_out(i);
        }
        sendlen = OUTMAX-1;
    }
        break;
    case MOTION_IO_SET_OUT:
    {
        bsp_set_out(rcv_buf[1],(GPIO_PinState)rcv_buf[2]);
        sendlen = 0;
    }
        break;
    case MOTION_GET_SYS_STATUS:
    {
        send_buf[4] = user_get_sys_status();
        sendlen = 1;
    }
    break;
    case MOTION_VENDER_CMD:
    {
        sendlen = VendorSpec.VenderCmd(&rcv_buf[1],&send_buf[4]);
    }
    break; 
    case MOTION_SET_COOR:
    {
        uint8_t axis = rcv_buf[1];
        int64_t coor = 0;
        memcpy(&coor,&rcv_buf[2],sizeof(int64_t));
        mt_set_axis_coor(axis,coor);
    }
    break;
    case MOTION_GET_MOVEINFO:
    {
        float t;
        uint8_t axis = rcv_buf[1];
        uint32_t speed = 0;
        memcpy(&speed,&rcv_buf[2],sizeof(uint32_t));
        int64_t coor = 0;
        memcpy(&coor,&rcv_buf[6],sizeof(int64_t));
        motion_axis_move_info(axis,speed,coor);
        memcpy(&send_buf[4],&axis_info[axis].move_info->acc_num,sizeof(int64_t));
        t = axis_info[axis].move_info->t_acc;
        memcpy(&send_buf[12],&t,sizeof(float));
        memcpy(&send_buf[16],&axis_info[axis].move_info->uin_num,sizeof(int64_t));
        t = axis_info[axis].move_info->t_uin;
        memcpy(&send_buf[24],&t,sizeof(float));
        memcpy(&send_buf[28],&axis_info[axis].move_info->dec_num,sizeof(int64_t));
        t = axis_info[axis].move_info->t_dec;
        memcpy(&send_buf[36],&t,sizeof(float));
        sendlen = 36;
    }
    break;
    case MOTION_GET_BOARD_INFO:
    {
        char *str = print_sys_time();
        memcpy(&send_buf[4],str,strlen(str));
        sendlen = strlen(str);
    }
    break;
    case MOTION_GET_NET_INFO:
		{
				uint16_t port = Server_PORT;
        memcpy(&send_buf[4],&netif_default->ip_addr,sizeof(uint32_t));
        memcpy(&send_buf[8],&port,sizeof(uint16_t));
        sendlen = 6;
		}
    break;
    default:
        break;
    }
    sendlen+=1;
    send_buf[0] = 0xaa;
    send_buf[1] = sendlen;
    send_buf[2] = sendlen>>8;
    send_buf[3] = rcv_buf[0];
    send_buf[sendlen + 4] = 0x55;

	return sendlen + 5;
}

void tcp_send_axis_move_end(uint8_t axis)
{
    uint8_t send_buf[16];
    pr_info("axis %d move finish %d coor %lld\n",axis,axis_info[axis].move_info->move_rsp,axis_info[axis].move_info->move_rsp_coor);

    send_buf[0] = 0xaa;
    send_buf[1] = 11;
    send_buf[2] = 0;
    send_buf[3] = MOTION_AXIS_MOVE_FINISH_INFO;
    send_buf[4] = axis;
    memcpy(&send_buf[5],&axis_info[axis].move_info->move_rsp_coor,sizeof(int64_t));
    send_buf[13] = axis_info[axis].move_info->move_rsp;
    send_buf[14] = 0x55;
    tcp_send_data(send_buf,15);
}
